#include <EEPROM.h>
const int addr = 0;

int myangle;     // 定义角度变量
int pulsewidth;  // 定义脉宽变量
// MG90S servo PWM pulse traveling
const int PWMRES_Min = 1;    // PWM Resolution 1
const int PWMRES_Max = 180;  // PWM Resolution 180
const int SERVOMIN = 150;
const int SERVOMAX = 600;

// Servos matrix
const int ALLMATRIX = 9;
const int ALLSERVOS = 8;

// Servo delay base time
const int BASEDELAYTIME = 15;  // 10 ms

// Backup servo value 存放各个舵机当前数值
int Running_Servo_POS[ALLMATRIX];

//const int Servo_Map[ALLSERVOS] = {8, 9, 10, 11, 4, 5, 6, 7};
const int Servo_Map[ALLSERVOS] = { 4, 5, 6, 7, 8, 9, 10, 11 };
/*************************************    此处舵机归零位置微调，修改数组中相应数值  *************************/
// 修正误差值
int Servo_Bias_Correction[ALLSERVOS] = { 100, 100, 100, 100, 100, 100, 100, 100 };
/******************************************************************************************************/

// Servo zero position 归零位置
const int Servo_Act_0[] PROGMEM = { 60, 45, 135, 60, 60, 135, 45, 60, 500 };

// Standby 站立
const int Servo_Prg_1_Step = 2;
const int Servo_Prg_1[][ALLMATRIX] PROGMEM = {
  { 90, 90, 90, 90, 90, 90, 90, 90, 500 },    // servo center point
  { 130, 90, 90, 130, 130, 90, 90, 130, 500 },  // standby
};

// Forward 前行
const int Servo_Prg_2_Step = 11;
const int Servo_Prg_2[][ALLMATRIX] PROGMEM = {
  { 110, 90, 90, 110, 110, 90, 90, 110, 100 },   // standby
  { 80, 90, 90, 110, 110, 90, 45, 80, 100 },   // leg1,4 up; leg4 fw
  { 110, 90, 90, 110, 110, 90, 45, 110, 200 },   // leg1,4 dn
  { 110, 90, 90, 80, 80, 90, 45, 110, 100 },     // leg2,3 up
  { 110, 45, 135, 80, 80, 90, 90, 110, 100 },    // leg1,4 bk; leg2 fw
  { 110, 45, 135, 110, 110, 90, 90, 110, 200 },  // leg2,3 dn
  { 80, 90, 135, 110, 110, 90, 90, 80, 100 },  // leg1,4 up; leg1 fw
  { 80, 90, 90, 110, 110, 135, 90, 80, 100 },  // leg2,3 bk
  { 110, 90, 90, 110, 110, 135, 90, 110, 200 },  // leg1,4 dn
  { 110, 90, 90, 110, 80, 135, 90, 110, 100 },   // leg3 up
  { 110, 90, 90, 110, 110, 90, 90, 110, 200 },   // leg3 fw dn
};

// Backward 后退
const int Servo_Prg_3_Step = 11;
const int Servo_Prg_3[][ALLMATRIX] PROGMEM = {
  { 110, 90, 90, 110, 110, 90, 90, 110, 100 },   // standby
  { 80, 45, 90, 110, 110, 90, 90, 80, 100 },   // leg4,1 up; leg1 fw
  { 110, 45, 90, 110, 110, 90, 90, 110, 200 },   // leg4,1 dn
  { 110, 45, 90, 80, 80, 90, 90, 110, 100 },     // leg3,2 up
  { 110, 90, 90, 80, 80, 135, 45, 110, 100 },    // leg4,1 bk; leg3 fw
  { 110, 90, 90, 110,110, 135, 45, 110, 200 },  // leg3,2 dn
  { 80, 90, 90, 110,110, 135, 90, 80, 100 },  // leg4,1 up; leg4 fw
  { 80, 90, 135, 110,110, 90, 90, 80, 100 },  // leg3,2 bk
  { 110, 90, 135, 110,110, 90, 90, 110, 200 },  // leg4,1 dn
  { 110, 90, 135, 80,110, 90, 90, 110, 100 },   // leg2 up
  { 110, 90, 90, 110, 110, 90, 90, 110, 200 },   // leg2 fw dn
};

// Turn left 左转
const int Servo_Prg_6_Step = 8;
const int Servo_Prg_6[][ALLMATRIX] PROGMEM = {
  { 110, 90, 90, 110, 110, 90, 90, 110, 150 },      // standby
  { 90, 90, 90, 110, 110, 90, 90, 90, 150 },      // leg1,4 up
  { 90, 135, 90, 110, 110, 90, 135, 90, 150 },    // leg1,4 turn
  { 110, 135, 90, 110, 110, 90, 135, 110, 150 },    // leg1,4 dn
  { 110, 135, 90, 90, 90, 90, 135, 110, 150 },      // leg2,3 up
  { 110, 135, 135, 90, 90, 135, 135, 110, 150 },    // leg2,3 turn
  { 110, 135, 135, 110, 110, 135, 135, 110, 150 },  // leg2,3 dn
  { 110, 90, 90, 110, 110, 90, 90, 110, 150 },      // leg1,2,3,4 turn
};

// Turn right 右转
const int Servo_Prg_7_Step = 8;
const int Servo_Prg_7[][ALLMATRIX] PROGMEM = {
  { 110, 90, 90, 110, 110, 90, 90, 110, 150 },  // standby
  { 110, 90, 90, 90, 90, 90, 90, 110, 150 },    // leg2,3 up
  { 110, 90, 45, 90, 90, 45, 90, 110, 150 },    // leg2,3 turn
  { 110, 90, 45, 110, 110, 45, 90, 110, 150 },  // leg2,3 dn
  { 90, 90, 45, 110, 110, 45, 90, 90, 150 },  // leg1,4 up
  { 90, 45, 45, 110, 110, 45, 45, 90, 150 },  // leg1,4 turn
  { 110, 45, 45, 110, 110, 45, 45, 110, 150 },  // leg1,4 dn
  { 110, 90, 90, 110, 110, 90, 90, 110, 150 },  // leg1,2,3,4 turn
};

// Lie 趴地
const int Servo_Prg_8_Step = 1;
const int Servo_Prg_8[][ALLMATRIX] PROGMEM = {
  { 65, 90, 90, 65, 65, 90, 90, 65, 500 },  // leg1,4 up
};

void Servo_PROGRAM_Zero() {
  for (int Index = 0; Index < ALLMATRIX; Index++) {
    Running_Servo_POS[Index] = pgm_read_byte_near(&Servo_Act_0[Index]) + Servo_Bias_Correction[Index] - 100;
  }
  for (int i = 0; i <= 20; i++) {
    for (int iServo = 0; iServo < ALLSERVOS; iServo++) {
      servopulse_zero(Servo_Map[iServo], Running_Servo_POS[iServo]);
    }
  }
}

void servopulse(int servopin, int myangle) {
  pulsewidth = (myangle * 11) + 500;
  digitalWrite(servopin, HIGH);
  delayMicroseconds(pulsewidth);
  digitalWrite(servopin, LOW);
}

void servopulse_zero(int servopin, int myangle) {
  pulsewidth = (myangle * 11) + 500;
  digitalWrite(servopin, HIGH);
  delayMicroseconds(pulsewidth);
  digitalWrite(servopin, LOW);
  delay(20 - pulsewidth / 1000);
}

void Set_PWM_to_Servo(int iServo, int iValue) {
  int NewPWM = iValue + Servo_Bias_Correction[iServo] - 100;
  servopulse(Servo_Map[iServo], NewPWM);
}

void Servo_Play_run(int Array[][ALLMATRIX], int iSteps) {
    int INT_TEMP_A, INT_TEMP_B, INT_TEMP_C, NewPWM;
    for (int index1 = 0; index1 < iSteps; index1++) {
        int TotalTime = pgm_read_word_near(&Array[index1][ALLMATRIX - 1]);
        int DelayTime = TotalTime / BASEDELAYTIME;  // 减少步进次数
        for (int timeloop = 1; timeloop <= DelayTime; timeloop++) {
            for (int index2 = 0; index2 < ALLSERVOS; index2++) {
                INT_TEMP_A = Running_Servo_POS[index2];  // 当前舵机位置
                INT_TEMP_B = pgm_read_word_near(&Array[index1][index2]);  // 目标舵机位置

                if (INT_TEMP_A == INT_TEMP_B) {
                    NewPWM = INT_TEMP_B;
                } else if (INT_TEMP_A > INT_TEMP_B) {
                    INT_TEMP_C = INT_TEMP_A - INT_TEMP_B;
                    NewPWM = INT_TEMP_A - (INT_TEMP_C * timeloop / DelayTime);
                } else {
                    INT_TEMP_C = INT_TEMP_B - INT_TEMP_A;
                    NewPWM = INT_TEMP_A + (INT_TEMP_C * timeloop / DelayTime);
                }
                Set_PWM_to_Servo(index2, NewPWM);
            }
        }
        for (int Index = 0; Index < ALLMATRIX; Index++) {
            Running_Servo_POS[Index] = pgm_read_word_near(&Array[index1][Index]);
        }
    }
}

void setup() {
  Serial.begin(9600);
  for (int i = addr; i < 8; i++) {
    Servo_Bias_Correction[i] = EEPROM.read(i);
    delay(100);
    if (Servo_Bias_Correction[i] > 150 || Servo_Bias_Correction[i] < 50) {
      Servo_Bias_Correction[i] = 100;
    }
  }
  Servo_PROGRAM_Zero();
  delay(500);
}

enum ActionState {
  IDLE, FORWARD, BACKWARD
};

ActionState currentState = IDLE;

void loop() {
  if (Serial.available() > 0) {
    String data = "";
    while (Serial.available() > 0) {
      data += char(Serial.read());
      delay(2);
    }
    if (data.length() > 0) {
      switch (data.charAt(0)) {
        case 'S':
          currentState = IDLE;
          Servo_Play_run(Servo_Prg_1, Servo_Prg_1_Step);
          break;
        case 'G':
          if (currentState == FORWARD) {
            currentState = IDLE;  // 如果当前已经在前进，再次收到G停止前进
          } else {
            currentState = FORWARD;  // 否则开始前进
          }
          break;
        case 'B':
          if (currentState == BACKWARD) {
            currentState = IDLE;  // 如果当前已经在后退，再次收到B停止后退
          } else {
            currentState = BACKWARD;  // 否则开始后退
          }
          break;
        case 'l':
          currentState = IDLE;
          Servo_Play_run(Servo_Prg_6, Servo_Prg_6_Step);
          break;
        case 'r':
          currentState = IDLE;
          Servo_Play_run(Servo_Prg_7, Servo_Prg_7_Step);
          break;
        case 'P':
          currentState = IDLE;
          Servo_Play_run(Servo_Prg_8, Servo_Prg_8_Step);
          break;
        case 'C':
          int servo_id = 0;
          if (data.length() == 3 && data.charAt(1) == '1') {
            switch (data.charAt(2)) {
              case '0':
                Servo_Bias_Correction[4] -= 2;
                servo_id = 4;
                break;
              case '1':
                Servo_Bias_Correction[5] += 2;
                servo_id = 5;
                break;
              case '2':
                Servo_Bias_Correction[5] -= 2;
                servo_id = 5;
                break;
              case '3':
                Servo_Bias_Correction[6] += 2;
                servo_id = 6;
                break;
              case '4':
                Servo_Bias_Correction[6] -= 2;
                servo_id = 6;
                break;
              case '5':
                Servo_Bias_Correction[7] += 2;
                servo_id = 7;
                break;
              case '6':
                Servo_Bias_Correction[7] -= 2;
                servo_id = 7;
                break;
              default:
                break;
            }
          } else {
            switch (data.charAt(1)) {
              case '1':
                Servo_Bias_Correction[0] += 2;
                servo_id = 0;
                break;
              case '2':
                Servo_Bias_Correction[0] -= 2;
                servo_id = 0;
                break;
              case '3':
                Servo_Bias_Correction[1] += 2;
                servo_id = 1;
                break;
              case '4':
                Servo_Bias_Correction[1] -= 2;
                servo_id = 1;
                break;
              case '5':
                Servo_Bias_Correction[2] += 2;
                servo_id = 2;
                break;
              case '6':
                Servo_Bias_Correction[2] -= 2;
                servo_id = 2;
                break;
              case '7':
                Servo_Bias_Correction[3] += 2;
                servo_id = 3;
                break;
              case '8':
                Servo_Bias_Correction[3] -= 2;
                servo_id = 3;
                break;
              case '9':
                Servo_Bias_Correction[4] += 2;
                servo_id = 4;
                break;
              default:
                break;
            }
          }
          EEPROM.write(servo_id, Servo_Bias_Correction[servo_id]);
          for (int Index = 0; Index < ALLMATRIX; Index++) {
            Running_Servo_POS[Index] = pgm_read_byte_near(&Servo_Act_0[Index]) + Servo_Bias_Correction[Index] - 100;
          }
          for (int i = 0; i <= 5; i++) {
            for (int iServo = 0; iServo < ALLSERVOS; iServo++) {
              servopulse(Servo_Map[iServo], Running_Servo_POS[iServo]);
            }
          }
          data = "";
          break;
        default:
          break;
      }
    }
  }

  // 根据当前状态执行循环动作
  if (currentState == FORWARD) {
    Servo_Play_run(Servo_Prg_2, Servo_Prg_2_Step);
  } else if (currentState == BACKWARD) {
    Servo_Play_run(Servo_Prg_3, Servo_Prg_3_Step);
  }
}
